#!coding:utf-8
import numpy as np
import time
# from src.IMU import IMU
from hexapod_controller.hardware_interface import HardwareInterface
# from hexapod_controller.hardware_config import NEUTRAL_ANGLE_DEGREES

NEUTRAL_ANGLE_DEGREES = np.array(
[[0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]
,dtype = 'float')

from hexapod_controller.kinematics import Solver
from hexapod_controller.controller import Controller
from hexapod_controller.config import Configuration, State, Command, BehaviorState

import signal

# 自定义信号处理函数
def my_handler(signum, frame):
    print("进程终止")
    exit()


# 设置相应信号处理的handler
signal.signal(signal.SIGINT, my_handler)
signal.signal(signal.SIGTERM, my_handler)


from apriltag_detector import ApriltagDetector
from picamera2 import Picamera2
import cv2

picam = Picamera2(0)
conf = picam.create_still_configuration(main={"size": (640, 480), 'format': 'RGB888'})
picam.configure(conf)
picam.start()
cv2.namedWindow('frame', cv2.WINDOW_NORMAL)
cv2.setWindowProperty('frame', cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN)


command = Command()
state = State()
config = Configuration()

def main(use_imu=False):
    """Main program
    """
    # Create config
    solver = Solver(config)
    hardware_interface = HardwareInterface(NEUTRAL_ANGLE_DEGREES)

    # Create imu handle
    if use_imu:
        imu = IMU(port="/dev/ttyACM0")
        imu.flush_buffer()

    # Create controller and user input handles
    controller = Controller(
        config,
        solver.inverse_kinematics_body,
    )

    last_loop = time.time()

    print("Summary of gait parameters:")
    print("overlap time: ", config.overlap_time)
    print("swing time: ", config.swing_time)
    print("z clearance: ", config.z_clearance)
    print("x shift: ", config.x_shift)

    # exit()

    while True:
        now = time.time()
        if now - last_loop < config.dt:
            continue
        last_loop = time.time()

        # Read imu data. Orientation will be None if no data was available
        quat_orientation = (
            imu.read_orientation() if use_imu else np.array([1, 0, 0, 0])
        )
        state.quat_orientation = quat_orientation

        # Step the controller forward by dt
        try:
            controller.run(state, command)
        except Exception as e:
            print(e)
        hardware_interface.set_actuator_postions(state.joint_angles)
        # print()


def detect():
    detector = ApriltagDetector()

    while True:
        image = picam.capture_array()
        image = cv2.flip(image, -1)
        detector.detect(image)
        if detector.isDetected(1):
            state.behavior_state = BehaviorState.TROT
            x, y, z = detector.tag[2]
            print(x, y, z)
            # 根据横向偏移调整转向
            if abs(x) > 5:
                command.yaw_rate = config.max_yaw_rate*x/abs(x)/2
            else:
                command.yaw_rate = 0

            # 根据距离调整速度
            if z < 15:
                command.horizontal_velocity = np.array([-0.15, 0.0])
            elif z > 25:
                command.horizontal_velocity = np.array([0.25, 0.0])
            else:
                command.horizontal_velocity = np.array([0.0, 0.0])
        else:
            # 没检测到标签就让机器人停下来
            # state.behavior_state = BehaviorState.REST
            command.horizontal_velocity = np.array([0.0, 0.0])
            command.yaw_rate = 0

        cv2.imshow("frame", image)
        c = cv2.waitKey(4)
        if c == ord('q'):
            break
    picam.stop()
    cv2.destroyAllWindows()

if __name__ == '__main__':
    import threading
    thread  = threading.Thread(target=main)
    # 设置守护线程, 主线程结束后自动退出
    thread.daemon = True
    thread.start()
    detect()
